#include "App_Communication.h"

uint8_t count = 0;
uint8_t rx_buffer[TX_PLOAD_WIDTH];

// 处理通信
Com_State App_Communication_Data(Remote_struct *remote)
{
    memset(rx_buffer, 0, sizeof(rx_buffer));
    // 接收数据
    Int_SI24R1_RxPacket(rx_buffer);
    // 处理头校验
    if (rx_buffer[0] != HEAD_CHECK0 || rx_buffer[1] != HEAD_CHECK1 || rx_buffer[2] != HEAD_CHECK2)
    {

        return COM_ERROR;
    }

    // 处理数据
    remote->THR = rx_buffer[3] | (rx_buffer[4] << 8);
    remote->YAW = rx_buffer[5] | (rx_buffer[6] << 8);
    remote->PITCH = rx_buffer[7] | (rx_buffer[8] << 8);
    remote->ROLL = rx_buffer[9] | (rx_buffer[10] << 8);

    remote->SHUT_DOWN = rx_buffer[11];
    remote->FIX_HEIGHT = rx_buffer[12];
    remote->MPU6050_RESET = rx_buffer[13];

    // COM_DEBUG_PRINTFln(": %d %d %d %d %d %d", remote->THR, remote->YAW, remote->PITCH, remote->ROLL, remote->SHUT_DOWN, remote->FIX_HEIGHT);

    return COM_OK;
}
// 处理遥控状态
void App_Communication_Control(Control_status *control_status, Com_State com_state)
{
    if (com_state == COM_OK)
    {
        *control_status = CONNECT;
        count = 0;
    }
    else
    {
        count++;
        if (count > 50)
        {
            *control_status = DISCONNECT;
            count = 0;
        }
    }
}

Throttle_State thr_state = FREE;
uint32_t enter_MAX_time = 0;
uint32_t enter_min_time = 0;
// 解锁状态
/**
 * @brief 解锁状态
 *
 * @param remote 遥控器数据
 * @return Com_State 解锁状态
 * @retval COM_OK 解锁成功
 * @retval COM_ERROR 解锁失败
 *
 */
Com_State App_Communication_UnLock(Remote_struct *remote)
{
    // 根据thr_state的值，执行不同的操作
    switch (thr_state)
    {
    case FREE:
        // 如果THR的值大于等于1800，则将thr_state设置为MAX，并记录当前时间
        if (remote->THR >= 1800)
        {
            thr_state = MAX;
            enter_MAX_time = xTaskGetTickCount();
        }

        break;
    case MAX:

        // 如果THR的值小于1800，则判断当前时间与进入MAX状态的时间差是否大于1000ms
        if (remote->THR < 1800)
        {
            if (xTaskGetTickCount() - enter_MAX_time > 1000)

            {
                // 如果大于1000ms，则将thr_state设置为LEAVE_MAX
                thr_state = LEAVE_MAX;
            }
            else
            {
                // 如果小于1000ms，则将thr_state设置为FREE
                thr_state = FREE;
            }
        }
        break;
    case MIN:
        // 如果当前时间与进入MIN状态的时间差小于1000ms，且THR的值大于200，则将thr_state设置为FREE
        if (xTaskGetTickCount() - enter_min_time < 1000)
        {
            if (remote->THR > 200)
            {
                thr_state = FREE;
            }
        }
        else
        {
            // 否则，将thr_state设置为UNLOCK
            thr_state = UNLOCK;
        }
        break;
    case LEAVE_MAX:
        // 如果THR的值小于200，则将thr_state设置为MIN，并记录当前时间
        if (remote->THR < 200)
        {
            thr_state = MIN;
            enter_min_time = xTaskGetTickCount();
        }

        break;

    default:
        break;
    }

    // 如果thr_state为UNLOCK，则返回COM_OK
    if (thr_state == UNLOCK)
    {

        return COM_OK;
    }

    // 否则，返回COM_ERROR
    return COM_ERROR;
}
extern uint16_t height;
extern uint16_t current_height;
// 处理飞控状态
/**
 * @brief 处理飞控状态
 *
 * @param fly_status 飞控状态
 * @param control_status 遥控状态
 * @param remote 遥控器数据
 */
void App_Communication_Fly(Fly_status *fly_status, Control_status control_status, Remote_struct *remote)
{

    switch (*fly_status)
    {
    case IDLE:
        if (App_Communication_UnLock(remote) == COM_OK)
        {
            *fly_status = NORMAL;
        }

        break;
    case NORMAL:
        if (remote->FIX_HEIGHT == 1)
        {
            *fly_status = FIX_HEIGHT;
            // 获得高度值
            height = current_height;
        }
        if (control_status == DISCONNECT)
        {
            *fly_status = FAULT;
        }

        break;
    case FIX_HEIGHT:
        if (remote->FIX_HEIGHT == 1)
        {
            *fly_status = NORMAL;
        }
        if (control_status == DISCONNECT)
        {
            *fly_status = FAULT;
        }

        break;
    case FAULT:
        // COM_DEBUG_PRINTFln("FAULT");
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        *fly_status = IDLE;
        thr_state = FREE;
        break;
    default:
        break;
    }
}
